import time
import random
import glob
import os
import sys
try:
    sys.path.append(glob.glob('C:/carla/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass
import carla
import pyproj
import math
import socket
import json

# Connect to client
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)

# Get World and Actors
world = client.get_world()
current_map = world.get_map()

actor_list = []

acc_lat = {}
acc_lon = {}
acc_yaw = {}

# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps="WGS84")


def spawn_vehicle():
    blueprint_library = world.get_blueprint_library()
    for _ in range(0, 10):
        transform = random.choice(world.get_map().get_spawn_points())
        bp = random.choice(blueprint_library.filter('vehicle.audi.tt'))
        npc = world.try_spawn_actor(bp, transform)
        if npc is not None:
            actor_list.append(npc)
            npc.set_autopilot(True)

def destroy_vehicle():
    print('destroying actors')
    client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
    print('done.')

def xy2gps(x, y, elevation):
    dis = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
    azm = math.degrees(math.atan2(-y, x))
    # 设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768, 43.8334643, azm, dis)
    return lon, lat, elevation


def getGps(actor):
    t = actor.get_transform()
    vid = actor.id
    lon, lat, elevation = xy2gps(-t.location.y, -t.location.x, t.location.z)
    lat = int(lat * 10000000)
    lon = int(lon * 10000000)
    acclat = 0
    acclon = 0
    if acc_lat.__contains__(vid):
        acclat = lat - acc_lat[vid]
    if acc_lon.__contains__(vid):
        acclon = lon - acc_lon[vid]

    acc_lat[actor.id] = lat
    acc_lon[actor.id] = lon
    return lat, lon, acclat, acclon


def getSpeed(actor):
    v = actor.get_velocity()
    speed = 3.6 * math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2)
    return int(speed)


def getHeading(actor):
    yaw = actor.get_transform().rotation.yaw
    if yaw < -90:
        yaw = 450 + yaw
    else:
        yaw = 90 + yaw
    accyaw = 0
    if acc_yaw.__contains__(actor.id):
        accyaw = yaw - acc_yaw[actor.id]
    acc_yaw[actor.id] = yaw
    return yaw, accyaw


def talker():
    # connect
    client = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) #声明socket类型，同时生成链接对象
    addr=('127.0.0.1',5555)

    actors = world.get_actors()

    while True:
        count = 0
        for actor in actors:
            if 'vehicle.audi.tt' == actor.type_id:
                BSM = {}
                BSM['idbuf'] = "000000" + str(actor.id)
                BSM['lat'], BSM['lon'], BSM['acclat'], BSM['acclon'] = getGps(actor)
                BSM['TransmissionState'] = actor.get_control().gear
                BSM['speed'] = getSpeed(actor)
                BSM['heading'], BSM['accyaw'] = getHeading(actor)
                BSM['accvert'] = 0
                serialData = json.dumps(BSM)
                client.sendto(serialData.encode('utf-8'),addr)
                print(serialData)
        time.sleep(0.5)


if __name__ == '__main__':
    try:
        spawn_vehicle()
        talker()
    finally:
        destroy_vehicle()
